////////////////////////////////////
// version 0.9.1 de Roby
////////////////////////////////////
#include <Servo.h>
#include <Wire.h>

// globales
unsigned long _rapport_turret_last = 0;
unsigned long _rapport_body_last = 0;
unsigned long _rapport_sensors_last = 0;
unsigned long _setting_rapport_stepping = 1;
unsigned long _setting_rapport_turret_interval = 1000/16;
unsigned long _setting_rapport_body_interval = 1000/16;
unsigned long _setting_rapport_sensors_interval = 1000/32;
boolean _send_status = false;
boolean _enable_debug = false;
boolean _send_invalid_cmd = true;

// target parameters
const unsigned int _target_type_none = 0;
const unsigned int _target_type_move = 1;
const unsigned int _target_type_rotate = 2;
boolean _target_mode_enable = false;
unsigned int _target_type = _target_type_none;
int _target_step = 0;

// beam parameters
int _beam_count = 7;
int _beam_delay = 5;
int _beam_sensors = 1|2;
float _beam_precision = 0.10f; // 15% of variation

// compass parameters
float body_compass_bearing=0.0f;
float body_compass_pitch=0.0f;
float body_compass_roll=0.0f;

// distance paramaters
float body_lastmove_distance_left=0.0f;
float body_lastmove_distance_right=0.0f;

// motor parameters
boolean motor_running=false, motor_step_counting=false;

// turret settings
const int turret_ir_min = 25, turret_ir_max = 145;
const int turret_us_min = 6, turret_us_max = 550;
const int turret_low_min = 10, turret_low_mid = 80, turret_low_max = 170;
const int turret_high_min = 30, turret_high_mid = 90, turret_high_max = 105;

// turret parameters
int current_turret_position_low = turret_low_mid;
int current_turret_position_high = turret_high_mid;

//////////////
// setup
void setup(void)
{
  //Serial.begin(115200); // Set Baud Rate for BT
  Serial.begin(9600); // Set Baud Rate for COM
  Serial.println("Init...");
  Wire.begin();
  setup_motor();
  setup_encoders();
  setup_rangeswiches();
  setup_turret();
  delay(250);
  Serial.println("Ready...");
}

unsigned long _time_=0;

///////////////////////////////
// loop
void loop(void)
{
  check_body_compass();
  check_reach_target();
  check_commands();

  _time_ = millis();
  if(_send_status)
  {
    if(( _time_ - _rapport_turret_last) > (_setting_rapport_turret_interval*_setting_rapport_stepping))
    {
      send_status_turret();
      _rapport_turret_last = _time_;
    }
    if(( _time_ - _rapport_body_last) > (_setting_rapport_body_interval*_setting_rapport_stepping))
    {
      send_status_body();
      _rapport_body_last = _time_;
    }
    if(( _time_ - _rapport_sensors_last) > (_setting_rapport_sensors_interval*_setting_rapport_stepping))
    {
      send_status_hitsensors();
      _rapport_sensors_last = _time_;
    }
  }
}

